#line 1 "D:/Archivio/mikroC/tric-newsensors/lib_itg3200.c"
#line 8 "D:/Archivio/mikroC/tric-newsensors/lib_itg3200.c"
extern int gyro[3];

void ITG3200_init() {
 unsigned short temp;


 I2C1_Start();
 I2C1_Wr( 0xD2 );
 I2C1_Wr(0x3E);
 I2C1_Stop();
 I2C1_Start();
 I2C1_Wr( 0xD3 );
 temp = I2C1_Rd(0);
 I2C1_Stop();
 I2C1_Start();
 I2C1_Wr( 0xD2 );
 I2C1_Wr(0x3E);
 I2C1_Wr(((temp & 0xF1) | 0x03));
 I2C1_Stop();
 Delay_ms(50);


 I2C1_Start();
 I2C1_Wr( 0xD2 );
 I2C1_Wr(0x15);
 I2C1_Wr(0x01);
 I2C1_Stop();
 Delay_ms(10);


 I2C1_Start();
 I2C1_Wr( 0xD2 );
 I2C1_Wr(0x16);
 I2C1_Wr(0x1D);
 I2C1_Stop();
 Delay_ms(10);
}

void ITG3200_readGyro() {
 unsigned short cnt;
 unsigned short gyro_tmp[6];

 I2C1_Start();
 I2C1_Wr( 0xD2 );
 I2C1_Wr(0x1D);
 I2C1_Stop();
 I2C1_Start();
 I2C1_Wr( 0xD3 );
 for (cnt=0; cnt<6; cnt++) {
 char ack;
 ack = (cnt<5);
 gyro_tmp[cnt] = I2C1_Rd(ack);
 }
 I2C1_Stop();

 gyro[0] = gyro_tmp[0];
 gyro[0] = gyro[0] << 8;
 gyro[0] |= gyro_tmp[1];
 gyro[1] = gyro_tmp[2];
 gyro[1] = gyro[1] << 8;
 gyro[1] |= gyro_tmp[3];
 gyro[2] = gyro_tmp[4];
 gyro[2] = gyro[2] << 8;
 gyro[2] |= gyro_tmp[5];
}
